//------------------------------------------------------------------------------
//  <copyright file="WorkingRegisterMessageManaged.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#pragma once
#include <ClientMessages.h>

using namespace System;

namespace Microsoft 
{
    namespace Robotics
    {
        namespace Hardware
        {
            namespace KukaLBR
            {
                /// <summary>
                /// Managed version of Working registers.
                /// Register values are copied from the corresponding unmanaged versions
                /// </summary>
                public ref class WorkingRegisterMessageManaged
                {
                public:
                    /// <summary>
                    /// The controller strategy as defined by KRL (Kuka).
                    /// Determines type of control.  Not all command registers
                    /// have an effect in all control modes.  
                    /// </summary>
                    double ControllerStrategyCommandRegister;

                    /// <summary> Commanded joint poisitions.  </summary>
                    array<double>^ JointPositionCommandRegister;

                    /// <summary> Commanded Cartesian poisitions.  </summary>
                    array<double>^ CartesianPositionCommandRegister;

                    /// <summary> Commanded joint torques.  </summary>
                    array<double>^  JointTorqueCommandRegister;

                    /// <summary> Commanded force/torque at the tool control point.  </summary>
                    array<double>^  TCPForceTorqueCommandRegister;

                    /// <summary> Commanded joint stiffness.  </summary>
                    array<double>^  JointStiffnessCommandRegister;

                    /// <summary> Commanded joint damping.  </summary>
                    array<double>^  JointDampingCommandRegister;

                    /// <summary> Commanded Cartesian stiffness.  </summary>
                    array<double>^  CartesianStiffnessCommandRegister;

                    /// <summary> Commanded Cartesian damping.  </summary>
                    array<double>^  CartesianDampingCommandRegister;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar1Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar2Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar3Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar4Register;

                    /// <summary> General purpose scalar.  </summary>
                    double Scalar5Register;

					/// <summary> General purpose scalar.  </summary>
					double Scalar6Register;

					/// <summary> General purpose scalar.  </summary>
					double Scalar7Register;

					/// <summary> General purpose scalar.  </summary>
					double Scalar8Register;

					/// <summary> General purpose 6-dim vector.  </summary>
					array<double>^  CartVec1Register;

                    /// <summary> General purpose 6-dim vector.  </summary>
                    array<double>^  CartVec2Register;

                    /// <summary> General purpose 6-dim vector.  </summary>
                    array<double>^  CartVec3Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec1Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec2Register;

                    /// <summary> General purpose vector with dimension equal to number of joints.  </summary>
                    array<double>^  JointVec3Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame1Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame2Register;

					/// <summary> General purpose 3x4 homogenous transform (last row is assumed to be [0 0 0 1]).  Row-major order. </summary>
					array<double>^  Frame3Register;

					/// <summary> Number of the state that the current controller is in. </summary>
					double CurrentControllerStateNumber;

                    /// <summary> Initializes a new instance of the <c>WorkingRegisterMessageManaged</c> class. </summary>
                    WorkingRegisterMessageManaged();

                    /// <summary> Copy to an unmanaged working register message </summary>
                    void CopyToUnmanaged(KukaWorkingRegisterMessage *pMessage);

					/// <summary>
					/// Copy from an array of arrays containing register values.
					/// Registers must be in order given in <c>KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusWorkingRegisters</c>
					/// </summary>
					void CopyFromArrayOfArrays(array<array<double>^>^ registers);

                private:
                    /// <summary> Allocate the dynamic arrays </summary>
                    void Initialize();

                    /// <summary>Copy from managed array to unmanaged array. Assumed to be same length </summary>
                    template<class T>
                    bool CopyFromManagedArray( array<T>^ from, size_t byteCount, T *to);
                };
            }
        }
    }
}
